function u = policy2(weight, x, x_des, idx, du_window)
%POLICY1 Summary of this function goes here
%   Detailed explanation goes here

rpy = q_to_euler(x(idx.q));
rpy_des = q_to_euler(x_des(idx.q));
error_pos = express_vector_in_quat_frame(x(idx.ned,:) - x_des(idx.ned,:), x(idx.q));
error_vel = express_vector_in_quat_frame(x(idx.ned_dot,:) - x_des(idx.ned_dot,:), x(idx.q));

l1 = weight(1:7)*[tanh(error_pos(2)); error_pos(2); error_vel(2); rpy(1); du_window(1,:)'];
l2 = weight(8:15)*[1; tanh(error_pos(1)); error_pos(1); error_vel(1); rpy(2); du_window(2,:)'];
l3 = weight(16:20)*[x(idx.pqr(3),:); wrapToPi(rpy(3) - rpy_des(3)); du_window(3,:)'];
l4 = weight(21:26)*[tanh(error_pos(3)); error_pos(3); error_vel(3); du_window(4,:)'];


u = [weight(27:28)*[tanh(l1); l1]; weight(29:30)*[tanh(l2); l2]; weight(31:32)*[tanh(l3);l3]; weight(33:34)*[tanh(l4);l4]]; %;long, lat, collective, rudder

end

